#include <Maths/Constants.h>
#include "intersections.h"
#include "structs.h"


float ScalarTriple(Vector3Arg u, Vector3Arg v, Vector3Arg w)
{
    return Dot(Cross(u, v), w);
}


bool IntersectRayQuads(Vector3Arg p, Vector3Arg q, const Quad* quads, size_t quadCount)
{
    Vector3 pq = q - p;

    Vector3 a, b, c, d, n;
    Vector3 pa, pb, pc, pd, m;
    float t, dot, u, v, w;

    const Quad* i = quads;
    const Quad* e = quads + quadCount;
    while (i != e)
    {
        const Quad& quad = *i++;

        a = quad.a;
        b = quad.b;
        c = quad.c;
        n = quad.n;

        dot = -Dot(pq, n);
        if (dot <= 0)
            continue;

        pa = a - p;
        t = -Dot(pa, n);
        if (t < 0.001f)
            continue;
        if (t > dot - 0.001f)
            continue;

        pb = b - p;
        pc = c - p;
        m = Cross(pc, pq);
        v = Dot(pa, m);
        if (v >= 0)
        {
            u = Dot(pb, m);
            if (u > 0)
                continue;
            w = ScalarTriple(pq, pb, pa);
            if (w < 0)
                continue;
        }
        else
        {
            d = quad.d;
            pd = d - p;
            u = Dot(pd, m);
            if (u < 0)
                continue;
            w = ScalarTriple(pq, pa, pd);
            if (w < 0)
                continue;
        }

        return true;
    }

    return false;
}


size_t IntersectRayQuadsClosest(Vector3Arg p, Vector3Arg q, const Quad* quads, size_t quadCount)
{
    size_t minIndex = 0xffffffff;
    float minT = kMaxPosFloat;

    Vector3 pq = q - p;

    Vector3 a, b, c, d, n;
    Vector3 pa, pb, pc, pd, m;
    float t, dot, u, v, w;

    for (size_t i=0; i<quadCount; ++i)
    {
        const Quad& quad = quads[i];

        a = quad.a;
        b = quad.b;
        c = quad.c;
        n = quad.n;

        dot = -Dot(pq, n);
        if (dot <= 0)
            continue;

        pa = a - p;
        t = -Dot(pa, n);
        if (t < 0.0001f)
            continue;
        if (t > dot - 0.0001f)
            continue;

        pb = b - p;
        pc = c - p;
        m = Cross(pc, pq);
        v = Dot(pa, m);
        if (v >= 0)
        {
            u = -Dot(pb, m);
            if (u < 0)
                continue;
            w = ScalarTriple(pq, pb, pa);
            if (w < 0)
                continue;
        }
        else
        {
            d = quad.d;
            pd = d - p;
            u = Dot(pd, m);
            if (u < 0)
                continue;
            w = ScalarTriple(pq, pa, pd);
            if (w < 0)
                continue;
        }

        t /= dot;
        if (t < minT)
        {
            minT = t;
            minIndex = i;
        }
    }

    return minIndex;
}
